#include "Astar.h"
#include "SDLittleDog.h"
#include "SDSlowWalk.h"
#include "SDVectorOp.h"
#include "Node.h"
#include <iostream>
#include <iomanip>
#include <ctime>
#include <stdlib.h>
#include <cstdlib>
#include <math.h>
#include <vector>
#include <fstream>
#include <stdio.h>
#include <utility>
#include <stack>
#include <queue>
#include <set>

using namespace std;

Astar::Astar(float gx, float gy, SDSimulator *sim, SDSlowWalk* walkCont, SDLittleDog* littleD, float dd, Node* start_node){
    //initialize variables
  d = dd;
  simulator = sim;
  goalx = gx;
  goaly = gy;
  littleDog = littleD;
  walkController = walkCont;
  start = start_node;

  stableX = .1; //these are the offsets from the dog's center that make the dog the most "stable"
  stableY = .065;

  Q.push(start);
  tree = new Tree("./matlab/tree.txt");
  printf("Astar object constructed\n");
}

 Astar::~Astar(){
  printf("Astar object destroyed\n");
}

//Saves the outcome of a simulation for training/testing data for the classifier:
void Astar::SaveOutcome(Node * node, int outcome){
     ofstream trainFile("matlab/trainingzillionnolegscosts.txt", ios_base::app );
     //printf("Printing to training file \n");
     if(!trainFile){
        printf("Error opening the file!\n");
     }
     trainFile<<setprecision(8);
     trainFile<<fixed;
     
     // write full features to file
     //for(int i=0; i < Node::features; i++){
     for(int i=12; i < Node::features; i++){
          trainFile << node->costs[i] << " ";
     }
     // write only three costs to file
     //trainFile <<node->costs[22] << " " << node->costs[23] << " " << node->costs[24] << " ";
     trainFile << outcome << endl;
     trainFile.close();
}

//This method takes a reference to a node being considered for expansion and determines whether
//the step is possible - first by checking basic invalid conditions and second by simulation
bool Astar::ValidStep(Node* node){
  // check for start node
     if(node->parent == NULL){
        printf("validating start node.\n");
        return true;
     }
     if(node == start){
         printf("validating start node\n");
         return true;
     }

    // Simulate step
     SDSimulatorState *oldState = node->parent->state; 
     simulator->setState(oldState);
     //put the state into the new state
     SDSimulatorState* state = new SDSimulatorState();
     simulator->getState(state);
     simulator->setState(state);
     node->state = state;
     //move dog
     walkController->setStepPosition(node->moved_foot, node->moved_foot_x, node->moved_foot_y);
     littleDog->runTrial(state);
     simulator->getState(state);
     simulator->setState(state);

     //get the dog's new positions
     bduVec3f globalFeet[4];
     simulator->getGlobalFootPositions(globalFeet);

     //Record some costs:
     bduVec3f localFeet[4];
     simulator->getLocalFootPositions(localFeet);
     node->costs[0] = localFeet[0].n[0];
     node->costs[1] = localFeet[0].n[1];
     node->costs[2] = localFeet[0].n[2];
     node->costs[3] = localFeet[1].n[0];
     node->costs[4] = localFeet[1].n[1];
     node->costs[5] = localFeet[1].n[2];
     node->costs[6] = localFeet[2].n[0];
     node->costs[7] = localFeet[2].n[1];
     node->costs[8] = localFeet[2].n[2]; 
     node->costs[9] = localFeet[3].n[0];
     node->costs[10] = localFeet[3].n[1];
     node->costs[11] = localFeet[3].n[2];

     //this loop and following if statement check if the dog has flipped
     float zoff = 0;
     float x = node->moved_foot_x; 
     float y = node->moved_foot_y;
	 for(int z=0; z<4; z++){
        zoff += fabs(globalFeet[z].n[2] - simulator->getPointHeight(globalFeet[z].n[0], globalFeet[z].n[1]));
  	 }

     if( zoff > 0.05 ){
        if(saveCosts) SaveOutcome(node, 0);
	    return false;
	 }

     // check if the feet haven't slipped alot
     else if((x+ .05 > globalFeet[node->moved_foot].n[0]) && (x- .05 < globalFeet[node->moved_foot].n[0]) && (y+.05 > globalFeet[node->moved_foot].n[1]) && (y-.05 < globalFeet[node->moved_foot].n[1])){
		printf("(%f, %f) (%f, %f) (%f, %f), (%f, %f)- good one\n", globalFeet[0].n[0], globalFeet[0].n[1], globalFeet[1].n[0], globalFeet[1].n[1], globalFeet[2].n[0], globalFeet[2].n[1], globalFeet[3].n[0], globalFeet[3].n[1]);

        //set the foot position of the node equal to the actual foot position
        for(int a = 0; a < 4; a++){
            for(int b = 0; b < 3; b++){
                node->foot_pos[a].n[b] = globalFeet[a].n[b];
            }
        }

        //set the z-offset of the node
        node->zoff = zoff;

        //if were making training data, save the costs
        if(saveCosts) SaveOutcome(node, 1);
        return true;

     } 
     else{ //if the dog's feet slipped alot
        if(saveCosts) SaveOutcome(node, 0);
        return false;
	 }
     
     return false;
}




//This method takes a node pointer and tests whether it corresponds to a goal state
//It tests whether the distance from the dog's center is less than a gridsize
bool Astar::GoalTest(Node * testNode) {
     return sqrt( pow( testNode->center[0] - goalx ,2) + pow( testNode->center[1]-goaly ,2) ) < d;
}

//This method recursively backtracks the A* search tree from a goal-state node to populate
//the stack<step>, storing the dog's path.
void Astar::RetracePath(Node* currentNode){ //Recursively backtracks the A* search path
      if(currentNode->parent != NULL){
         path.push(step(currentNode));
         RetracePath(currentNode->parent);
      }
}

//This method takes a struct<step> pointer representing a path solotion for the dog and
//prints this path to a file. 
void Astar::SavePathToFile(const char* filename){
     ofstream pathFile(filename);
     pathFile<<setprecision(8);
     pathFile<<fixed;
     printf("Printing path file \n");
     if(!pathFile){
        printf("Error opening the file!\n");
     }
     while(!path.empty()){
        step nextStep = path.top();
        path.pop();
        pathFile << nextStep.foot << " " << setprecision(8)<<fixed<<nextStep.x << " " << nextStep.y << endl;
     }
}

//This function returns the x coordinate of a foot's new position given which foot it is 
//and the grid offset it has
float Astar::getXfoot(int movingFoot, int xOff){
    if(movingFoot == 0 || movingFoot == 1)
        return ( (goalx + stableX) + d*xOff );
    else 
        return ( (goalx - stableX) + d*xOff );

}

//This function returns the y coordinate of a foot's new position given which foot it is 
//and the grid offset it has
float Astar::getYfoot(int movingFoot, int yOff){
    if(movingFoot == 0 || movingFoot == 2)
        return ( (goaly + stableY) + d*yOff );
    else
        return ( (goaly - stableY) + d*yOff );

}

//This function computes the euclidean distance between two arrays of size 2
float Astar::dist(float* one, float* two){
    float diff[2] = {one[0] - two[0], one[1] - two[1]};
    return sqrt(pow(diff[0], 2) + pow(diff[1], 2));
}

//This function computes the length of an array of size 2
float Astar::length(float* arr){
    float zero[2] = {0, 0};
    return dist(arr, zero);
}

//This function gets the general purpose cost of a node
float Astar::getCostOf(Node* child){
    float cost = 0;
    float stable_cost = 0;
    float direction_cost = 0;
    float area_cost = 0;

    //These lines compute the cost for a node of how far off it's legs are from the "most stable" position
    stable_cost += sqrt( pow( (goalx + stableX) - child->foot_pos[0].n[0] , 2) + pow( (goaly + stableY) - child->foot_pos[0].n[1], 2) );
    stable_cost += sqrt( pow( (goalx + stableX) - child->foot_pos[1].n[0] , 2) + pow( (goaly - stableY) - child->foot_pos[1].n[1], 2) );
    stable_cost += sqrt( pow( (goalx - stableX) - child->foot_pos[2].n[0] , 2) + pow( (goaly + stableY) - child->foot_pos[2].n[1], 2) );
    stable_cost += sqrt( pow( (goalx - stableX) - child->foot_pos[3].n[0] , 2) + pow( (goaly - stableY) - child->foot_pos[3].n[1], 2) );

    //These 3 lines compute the offset of the moved foot from the mean of the other 3 feet
    int foot = child->moved_foot;
    float altmean = ( child->foot_pos[(foot+1)% 4].n[2] + child->foot_pos[(foot+2)% 4].n[2] + child->foot_pos[(foot+3)% 4].n[2] )/3;
    float altstep = fabs(altmean - child->foot_pos[foot].n[2]);

    //These lines compute the 2D offset of the moved foot from the mean of the other 3 feet
    float mean[2] = {( child->foot_pos[(foot+1)% 4].n[0] + child->foot_pos[(foot+2)% 4].n[0] + child->foot_pos[(foot+3)% 4].n[0] )/3,
                                  ( child->foot_pos[(foot+1)% 4].n[1] + child->foot_pos[(foot+2)% 4].n[1] + child->foot_pos[(foot+3)% 4].n[1] )/3};
    float otherFoot[2] = {child->foot_pos[foot].n[0], child->foot_pos[foot].n[1]};
    float diststep = dist(mean, otherFoot);

    //Get the zoff of the parent node
    float zoff = child->parent->zoff;

    //Calculate the direction cost - based on the direction the dog is facing compared to the direction of the goal from the center of the dog
    float goal[2] = {goalx, goaly};
    if(dist(child->center, goal) >= .01){
        float front[2] = {(child->foot_pos[0].n[0] + child->foot_pos[1].n[0])/2, (child->foot_pos[0].n[1] + child->foot_pos[1].n[1])/2}; //average position of front feet
        float back[2] = {(child->foot_pos[2].n[0] + child->foot_pos[3].n[0])/2, (child->foot_pos[2].n[1] + child->foot_pos[3].n[1])/2}; //average position of back feet
        float dogDirection[2] = {front[0] - back[0], front[1] - back[1]}; //dog's direction vector
        float normalizingFactor = length(dogDirection);
        dogDirection[0] /= normalizingFactor; dogDirection[1] /= normalizingFactor; //normalize the dog's direction vector
        float goalDirection[2] = {goal[0] - child->center[0], goal[1] - child->center[1]}; //goal's direction vector
        normalizingFactor = length(goalDirection);
        goalDirection[0] /= normalizingFactor; goalDirection[1] /= normalizingFactor; //normalize the goal's direction vector
        float dot = goalDirection[0]*dogDirection[0] + goalDirection[1]*dogDirection[1]; //calculates the dot-product of dog's and goal's direction vectors 
        direction_cost = 1 - dot;
    
    } else direction_cost = 0;

    //Calculate the 3-leg area cost - based on the cost of the "balancing legs" when the dog is stepping (excluding the leg the dog is using to balance)
    int movingFoot = child->moved_foot;
    float leg0[2] = {child->foot_pos[0].n[0], child->foot_pos[0].n[1]}; //x-y position of foot 0
    float leg1[2] = {child->foot_pos[1].n[0], child->foot_pos[1].n[1]}; //x-y position of foot 1
    float leg2[2] = {child->foot_pos[2].n[0], child->foot_pos[2].n[1]}; //x-y position of foot 2
    float leg3[2] = {child->foot_pos[3].n[0], child->foot_pos[3].n[1]}; //x-y position of foot 3
    float vector1[2]; // depicted by vertical lines or slashes (the type with fewer count) below
    float vector2[2]; // depicted by vertical lines or slashes (the type with greater count) below
    if(movingFoot == 0){
        vector1[0] = leg2[0] - leg1[0]; vector1[1] = leg2[1] - leg1[1]; //  0 1
        vector2[0] = leg3[0] - leg1[0]; vector2[1] = leg3[1] - leg1[1]; //    /||
    }else if(movingFoot == 1){ //                                            2 3
        vector1[0] = leg2[0] - leg0[0]; vector1[1] = leg2[1] - leg0[1]; //  0 1
        vector2[0] = leg3[0] - leg0[0]; vector2[1] = leg3[1] - leg0[1]; /* |||\\     */
    }else if(movingFoot == 2){ //                                            2 3
        vector1[0] = leg1[0] - leg3[0]; vector1[1] = leg1[1] - leg3[1]; //  0 1
        vector2[0] = leg0[0] - leg3[0]; vector2[1] = leg0[1] - leg3[1]; //  \\|||
    }else if(movingFoot == 3){ //                                            2 3
        vector1[0] = leg1[0] - leg2[0]; vector1[1] = leg1[1] - leg2[1]; //  0 1
        vector2[0] = leg0[0] - leg2[0]; vector2[1] = leg0[1] - leg2[1]; //  ||/
    } //                                                                     2 3
    float area = vector1[0]*vector2[1] - vector1[1]*vector2[0]; //determinant of [vector1, vector2] to calculate signed area
    area /= (0.0191506/4); //Scales the area down (this number is a good area to have)
    if(area < 0) area_cost = -50/area; //if area is negative then the foot opposite the moving foot is over the diagonal between the other two feet (dog's crossing its feet - BAAAAD)
    else area_cost = 1/area;

    //Weight the different cost functions
    cost = 1*stable_cost + 1*direction_cost + 1*area_cost;//+ .1*altstep + 1*diststep + 1*zoff;

    //Calculate some other distances between legs
    float legDist_front = dist(leg0, leg1);
    float legDist_back = dist(leg2, leg3);
    float legDist_left = dist(leg0, leg2);
    float legDist_right = dist(leg1, leg3);
    float legDist_diag_down = dist(leg0, leg3);
    float legDist_diag_up = dist(leg1, leg2);

    //Assign costs to child.
    for(int i=0;i<12;i++){
          child->costs[i] = 0;
    }
    
    child->costs[12] = legDist_front;
    child->costs[13] = legDist_back;
    child->costs[14] = legDist_left;
    child->costs[15] = legDist_right;
    child->costs[16] = legDist_diag_down;
    child->costs[17] = legDist_diag_up;
    child->costs[18] = child->moved_foot;
    child->costs[19] = altstep;
    child->costs[20] = diststep;
    child->costs[21] = zoff;
    child->costs[22] = stable_cost;
    child->costs[23] = direction_cost;
    child->costs[24] = area_cost;
    child->costs[25] = cost;
                        
    float treeproba = tree->getCost(child);
    if (treeproba>0.3){ //if the node is deemed feasable by the classifier
       return cost;
    }
    //if the node isn't deemed feasable by the classifier
    return cost*1000;

}


//This method pushes all the children of the node* current onto the priority queue Q
//It makes new nodes of all the possible moves the dog has where the feet are within 
//5cm of the optimal foot position from the center goal that the dog has
//It also has a hilarious name
void Astar::pushChildren(Node* current){

    int nodeOffset = (int)floor(.05/d + .01); //the .01 is for rounding errors

    for(int footToMove = 0; footToMove < 4; footToMove ++){ //cycling through which foot the dog should move
         current->moving_foot = footToMove; //this is unnecessary

        for(int i = -nodeOffset; i <= nodeOffset; i++){ //cycling through the grid of all points 5cm or less away
            for(int j = -nodeOffset; j <= nodeOffset; j++){ //from the dog's ideal foot position

                Node* child = new Node();
                child->foot_pos = new bduVec3f[4];
                for(int k = 0; k < 4; k++){ //this loop sets the dog's feet positions
                    if(footToMove != k){  
                        child->foot_pos[k] = current->foot_pos[k];
                    }else{ //set the dog's moving foot 
                        child->foot_pos[k].n[0] = getXfoot(footToMove, i);
                        child->foot_pos[k].n[1] = getYfoot(footToMove, j);
                        child->moved_foot = footToMove;
                        child->moved_foot_x = child->foot_pos[k].n[0];
                        child->moved_foot_y = child->foot_pos[k].n[1];
                        child->foot_pos[k].n[2] = simulator->getPointHeight(child->foot_pos[k].n[0], child->foot_pos[k].n[1]);
                    }
                }

                child->parent = current;
                child->center = FindNodeCenter(child);
                child->g = getCostOf(child);
                child->h = 0;
                child->f = child->g + child->h;
                Q.push(child);

            }
        }
    }

}


Node* Astar::runSearch(){

  Node* toReturn = NULL;
  int expandedCounter = 0;
  int expandedLimit = 1000;//maximum number of expanded nodes befors admitting failure

  while(!Q.empty()){
     Node* current = Q.top(); //Obtain next candidate for expansion
     Q.pop();
    
     expandedCounter++; //check if this astar has been going on for too long
     if(expandedCounter>expandedLimit){
        printf("More than %d nodes expanded. Aborting search!\n",expandedLimit);
        return NULL;
     }

     it=expanded.find(current);
     if(it==expanded.end()){ //Check to see if not expanded before

        if(ValidStep(current)){ //Check to see if feasible step

           if(GoalTest(current)){ //Check to see if goal state
              printf("\n GOOOOOAAL FOOOOOOUND!!!!!!!!: \n\n");
              printf("center: (%f, %f) \n", current->center[0], current->center[1]);
              toReturn = current;
              break;
           }

           expanded.insert(current); //mark as expanded node
           pushChildren(current); //push all of current's children onto the stack
        }
     }

  } 
  //end of Astar

  //Delete everything in the queue (so we aren't orphaning memory)
  if(!Q.empty()){
     for(Node* next = Q.top(); !Q.empty(); next = Q.top()){
        Q.pop();
        delete next;
     }
  }

  //toReturn will have the goal node if astar found one, or NULL if not
  printf("%d nodes expanded\n",expandedCounter);
  return toReturn;
}

